/**
 * @file lateral_control
 * @author yq
 * @brief 
 * @version 0.1
 * @date 24-3-18
 *
 * @copyright Copyright (c) 2024
 */
#ifndef CONTROLLER_LATERAL_CONTROL_H
#define CONTROLLER_LATERAL_CONTROL_H
#include <cmath>
#include <eigen3/Eigen/Dense>
#include <chrono>
#include <algorithm>
#include <fstream>
#include <iostream>

#include "control_utils.h"
#include "controller_node.h"
using Eigen::MatrixXd;

class LQR {
public:
    int maxIteration_;
    double eps;

    LQR(Eigen::Matrix <double, 3, 3> Q, Eigen::Matrix <double, 2, 2> R,
        double eps, int maxIter, VehicleConfig vehicle_config);


    void LatExecutiveControl(const perception_msgs::TrajectoryPoint trace_point,
                             const CarState& vehicle_state,
                             ControlData& control_data);

    Eigen::MatrixXd Solve(Eigen::Matrix<double, 3, 3> A, Eigen::Matrix<double, 3, 2>  B,
                          Eigen::Matrix<double, 3, 3>  Q, Eigen::Matrix<double, 2, 2>  R, bool& is_solve);

private:
    double l;
    double steer_ratio;
    double error_x_;
    double error_y_;
    double error_theta_;
    double trace_kappa_;
    double trace_delta_;
    double trace_theta_;
    double dt_;
    bool is_solve_;
    bool first_run_;
    std::chrono::steady_clock::time_point currentTime_;
    std::chrono::steady_clock::time_point lastTime_;
    Eigen::Matrix <double, 3, 3> Q_;
    Eigen::Matrix <double, 2, 2> R_;
    Eigen::Matrix <double, 3, 3> A_;
    Eigen::Matrix <double, 2, 2> B_;

    std::ofstream outFile;
};
#endif //CONTROLLER_LATERAL_CONTROL_H
